/*
 * ClientDaemon.cpp
 *
 *  Created on: Mar 25, 2012
 *      Author: Victor Nidens victornlj@gmail.com
 * 		Author:	Mikhail S. Ananyevskiy, PhD, msaipme@yandex.ru
 *
 *	This file is part of Cloud Mechatronic Laboratory (CMLab) software.
 *
 *	Cloud Mechatronic Laboratory (CMLab) for CMLab is free software: you can redistribute it and/or modify
 *	it under the terms of the GNU Lesser General Public License as published by
 *	the Free Software Foundation, either version 3 of the License, or
 *	(at your option) any later version.
 *
 *	Cloud Mechatronic Laboratory (CMLab) software is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU Lesser General Public License for more details.
 *
 *	You should have received a copy of the GNU Lesser General Public License
 *	along with Cloud Mechatronic Laboratory (CMLab). If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "ClientDaemon.h"
#include "string.h"
#include <iostream>
#include <signal.h>
#include <sys/wait.h>
#include <cstdlib>
#include <fcntl.h>
#include <stdio.h>
extern "C" {
	#include "nxtusb/nxt.h"
	#include "nxtusb/usbcomm.h"
}

ClientDaemon::ClientDaemon(const ClientConfig conf){
	memset(&config, 0, sizeof(ClientConfig));
	memcpy(&config, &conf, sizeof(ClientConfig));

	libusb_device_handle *deviceHandles[config.legoDevicesCount];
	DeviceSystemInfo nxtInfo;

	memset(&tcpSocket, 0, sizeof(tcpSocket));

	tcpSocket.sin_family = AF_INET;
	tcpSocket.sin_addr.s_addr = inet_addr(config.serverIP.c_str());
	tcpSocket.sin_port = htons(config.tcpPort);

	tcpSocketFD = socket(AF_INET, SOCK_STREAM/* | SOCK_NONBLOCK*/, IPPROTO_TCP);

	memset(&udpSocket, 0, sizeof(udpSocket));

	udpSocket.sin_family = AF_INET;
	udpSocket.sin_addr.s_addr = inet_addr(config.serverIP.c_str());
	udpSocket.sin_port = htons(config.udpPort);

	udpSocketFD = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP);

	if (initUSB()) {
		cerr << "Can't init USB library." << endl;
		return;
	}

	if (connectNXTDevices(deviceHandles, &config.legoDevicesCount)) {
		cerr << "Can't connect to NXT devices." << endl;
		return;
	}

	//fetch names of the connected NXT devices
	for (uint8 ind = 0; ind < config.legoDevicesCount; ind++) {
		memset(&nxtInfo, 0, sizeof(nxtInfo));
		nxtGetDeviceSystemInfo(&deviceHandles[ind], &nxtInfo);

		if (!nxtInfo.status) {
			connectedDevices[(char*) (nxtInfo.deviceName)] = deviceHandles[ind];
		}
	}

	initDBStorage();

}

int ClientDaemon::tcpConnect(){
	return connect(tcpSocketFD, (struct sockaddr *) &tcpSocket, sizeof(tcpSocket));
}

int ClientDaemon::tcpPing(){

	uint8 sendBuf[128];

	ExchangePacket pingPacket;

	memset(&pingPacket, 0, sizeof(pingPacket));

	constructCommand(&pingPacket.command, COMMAND_PING, "", 0, 0, PROTOCOL_SAME);

	fillSourceInfo(&pingPacket.source, config.ip, config.name, PROTOCOL_TCP);

	memcpy(sendBuf, &pingPacket, NETWORK_PACKET_SIZE);

	int sentBytes = write(tcpSocketFD, sendBuf, NETWORK_PACKET_SIZE);

	return (sentBytes == NETWORK_PACKET_SIZE) ? 0 : 1;

}

int ClientDaemon::udpPing(){
	uint8 sendBuf[128];

	ExchangePacket pingPacket;

	memset(&pingPacket, 0, sizeof(pingPacket));

	constructCommand(&pingPacket.command, COMMAND_PING, "", 0, 0, PROTOCOL_SAME);

	fillSourceInfo(&pingPacket.source, config.ip, config.name, PROTOCOL_UDP);

	memcpy(sendBuf, &pingPacket, NETWORK_PACKET_SIZE);

	int sentBytes = sendto(udpSocketFD,
							sendBuf, NETWORK_PACKET_SIZE,
							0,
							(struct sockaddr*) &udpSocket, sizeof(udpSocket));

	return (sentBytes == NETWORK_PACKET_SIZE) ? 0 : 1;
}

int ClientDaemon::initDBStorage(){

	if(config.savePacketStat){

		dbConnection = mysql_init(NULL);

		if(!mysql_real_connect(dbConnection,
								config.dbHost.c_str(),
								config.dbUser.c_str(),
								config.dbUserPassword.c_str(),
								config.dbName.c_str(),
								config.dbPort,
								NULL, 0)){
			cerr << "Can't connect to MySQL db" << endl;
			config.savePacketStat = false;
			return 1;
		}
	}

	return 0;
}

void ClientDaemon::savePacketStat(ExchangePacket *packet){

	//if statistics save flag is switched off then just return
	if(!config.savePacketStat)
		return;

	char buf[16] = {0};

	string insertQuery = "INSERT INTO " + config.dbTable +
						" (sendTimestampSec, sendTimestampUsec, receiveTimestampSec, receiveTimestampUsec, sourceName, sourceIP, commandId, commandName, nxtName)" +
						" values (";

	//send timestamp seconds
	sprintf(buf, "%llu", packet->source.timestampSec);
	insertQuery.append(buf);
	insertQuery.append(", ");

	//send timestamp microseconds
	memset(buf, 0, sizeof(buf));
	sprintf(buf, "%llu", packet->source.timestampUsec);
	insertQuery.append(buf);
	insertQuery.append(", ");

	//received timestamp seconds
	memset(buf, 0, sizeof(buf));
	sprintf(buf, "%lu", packet->receivedTimestamp.tv_sec);
	insertQuery.append(buf);
	insertQuery.append(", ");

	//received timestamp microseconds
	memset(buf, 0, sizeof(buf));
	sprintf(buf, "%lu", packet->receivedTimestamp.tv_usec);
	insertQuery.append(buf);
	insertQuery.append(", ");
	insertQuery.append("'");

	//source name
	insertQuery.append(packet->source.sourceName);
	insertQuery.append("', '");

	//ip
	sprintf(buf, "%i.%i.%i.%i", packet->source.sourceIp[0], packet->source.sourceIp[1], packet->source.sourceIp[2], packet->source.sourceIp[3]);
	insertQuery.append(buf);
	insertQuery.append("', ");

	//command id
	memset(buf, 0, sizeof(buf));
	sprintf(buf, "%i", packet->command.command);
	insertQuery.append(buf);
	insertQuery.append(", '");

	//command name
	insertQuery.append(commandName(packet->command.command));
	insertQuery.append("', '");

	//nxt name
	insertQuery.append(packet->deviceData.deviceName);
	insertQuery.append("')");

	int query_state = mysql_query(dbConnection, insertQuery.c_str());

	if (query_state != 0) {
		cerr << mysql_error(dbConnection) << endl;
		return;
	}

}

void ClientDaemon::exec(){
	daemonize();
}

void ClientDaemon::daemonize(){
//	int pid;
//	struct sigaction sa;

	cout << "::: Starting Client daemon :::\n" << flush;
//	pid = fork();

//	switch (pid) {
//	case 0:
//		setsid();
//		chdir("/");
//
//		close(0);
//		close(1);
//		close(2);
//
//		memset(&sa, 0, sizeof(sa));
//		sa.sa_handler = &sighandler;
//		sigaction(SIGCHLD, &sa, 0);

//		openlog("daemon2", 0, LOG_USER);
		mainloop();
//		closelog();
		commit();
		exit(0);

//	case -1:
//		cout << "[ERR]Couldn't start UDP server daemon" << endl;
//		break;
//
//	default:
//		cout << "OK. PID=" << pid << endl;
//		break;
//	}
}

void ClientDaemon::mainloop(){

	fd_set readset;
	timeval timeout;
	uint8 buffer[128];
	ExchangePacket receivedPacket;

	int connectRes = tcpConnect();
	if(connectRes == -1){
		//TODO: how to handle failed tcp connection??
		return;
	}

	makeNonblockingFD(tcpSocketFD);

	udpPing();
	tcpPing();

	announceDevices();

	timeout.tv_sec = 0;
	timeout.tv_usec = 1000;

	while(1){
		FD_ZERO(&readset);
		FD_SET(udpSocketFD, &readset);
		FD_SET(tcpSocketFD, &readset);

		if(select(max(udpSocketFD, tcpSocketFD) + 1, &readset, NULL, NULL, &timeout) < 0){
			break;
		}

		//check if we have something new in TCP socket
		if(FD_ISSET(tcpSocketFD, &readset)){
			cout << "[CLIENT TCP INCOME] Something new in client's TCP socket" << endl;

			memset(buffer, 0, sizeof(buffer));

			int received = read(tcpSocketFD, buffer, NETWORK_PACKET_SIZE);

			if(received <= 0){
				break;
				//client was disconnected
				//try to reconnect
				//TODO: reconnection code here
			}

			if(received == NETWORK_PACKET_SIZE){
				cout << "[CLIENT TCP PACK INCOME] New ExchangePacket in client's TCP socket" << endl;

				memset(&receivedPacket, 0, sizeof(receivedPacket));

				memcpy(&receivedPacket, buffer, NETWORK_PACKET_SIZE);

				gettimeofday(&receivedPacket.receivedTimestamp, NULL);

				receivedQueue.push_back(receivedPacket);

				savePacketStat(&receivedPacket);

			}
		}

		//check if we have something new in UDP socket
		if(FD_ISSET(udpSocketFD, &readset)){
			cout << "[CLIENT UDP INCOME] Something new in client's UDP socket" << endl;

			memset(buffer, 0, sizeof(buffer));

			int received = recvfrom(udpSocketFD, buffer, sizeof(buffer), 0, NULL, NULL);

			if(received == NETWORK_PACKET_SIZE){
				cout << "[CLIENT UDP PACK INCOME] New ExchangePacket in client's UDP socket" << endl;

				memset(&receivedPacket, 0, sizeof(receivedPacket));

				memcpy(&receivedPacket, buffer, NETWORK_PACKET_SIZE);

				gettimeofday(&receivedPacket.receivedTimestamp, NULL);

				receivedQueue.push_back(receivedPacket);

				savePacketStat(&receivedPacket);

			}

		}

		//now execute one command from pending command list]
		handleReceivedPacket();

		//keep alive all connected NXT devices
		keepAliveDevices();

	}

}

void ClientDaemon::commit(){
	close(tcpSocketFD);
	close(udpSocketFD);
}

void ClientDaemon::makeNonblockingFD(int fd){
	int flags = fcntl(fd, F_GETFL, 0);

	fcntl(fd, F_SETFL, flags | O_NONBLOCK);

}

void ClientDaemon::handleReceivedPacket(){

	if(receivedQueue.size() < 1){
		return;
	}

	vector<ExchangePacket>::iterator first = receivedQueue.begin();

	ExchangePacket pack = *first;
	ExchangePacket sendPack;
	uint8 buffer[128] = {0};

	libusb_device_handle *nxtHandler = 0;
	int sentBytes;

	receivedQueue.erase(first);

	switch(pack.command.command){
	case COMMAND_PING:
	case COMMAND_DEVICE_ANNOUNCE_RESPONSE:

		break;
	case COMMAND_READ_SENSOR_REQUEST:

		SensorState sensorData;

		if(connectedDevices.find(pack.deviceData.deviceName) == connectedDevices.end())
			return;

		nxtHandler = connectedDevices[pack.deviceData.deviceName];

		//if sensor is analog
		if(pack.deviceData.sensor != NXT_LOW_SPEED
				&& pack.deviceData.sensor != NXT_LOW_SPEED_9V
				&& pack.deviceData.sensor != NXT_HISPEED){

			autoInitSensor(&pack, &nxtHandler);

			nxtGetInputValues(&nxtHandler, pack.deviceData.port, &sensorData);

			//send response to host

			memset(&sendPack, 0, sizeof(sendPack));

			sendPack.deviceData.dataFormat = FORMAT_RAW;

			sendPack.deviceData.sensor = pack.deviceData.sensor;
			sendPack.deviceData.port = pack.deviceData.port;

			sendPack.deviceData.dataSize = sizeof(sensorData);

			memcpy(sendPack.deviceData.data, &sensorData, sizeof(sensorData));

		} else {
			//digital sensor

			DigitalSensorReadRespose digitalResponse;

			memset(&digitalResponse, 0, sizeof(digitalResponse));

#pragma pack(push, 1)
		struct{
			uint8 txlen;
			uint8 rxlen;
			uint8 txdata[3];
		} digitalCommand;
#pragma pack(pop)

			memset(&digitalCommand, 0, sizeof(digitalCommand));

			memcpy(&digitalCommand, pack.deviceData.data, sizeof(digitalCommand));

			nxtLSRequestResponse(&nxtHandler, pack.deviceData.port, digitalCommand.txlen, digitalCommand.rxlen, digitalCommand.txdata, &digitalResponse);

			memset(&sendPack, 0, sizeof(sendPack));

			sendPack.deviceData.dataFormat = FORMAT_RAW;

			sendPack.deviceData.sensor = pack.deviceData.sensor;
			sendPack.deviceData.port = pack.deviceData.port;

			sendPack.deviceData.dataSize = sizeof(digitalResponse);

			memcpy(sendPack.deviceData.data, &digitalResponse, sizeof(digitalResponse));

		}

		fillSourceInfo(&sendPack.source, config.ip, config.name, pack.command.responseProtocol);

		constructCommand(&sendPack.command, COMMAND_READ_SENSOR_RESPONSE, pack.source.sourceName, 0, 0, PROTOCOL_SAME);

		memcpy(sendPack.deviceData.deviceName, pack.deviceData.deviceName, sizeof(sendPack.deviceData.deviceName));

		memcpy(buffer, &sendPack, NETWORK_PACKET_SIZE);

		if(sendPack.source.sourceProtocol == PROTOCOL_TCP){
			sentBytes = send(tcpSocketFD, buffer, NETWORK_PACKET_SIZE, 0);

			if(sentBytes != NETWORK_PACKET_SIZE){
				//TODO: notify an error occurred
			}
		} else {
			sentBytes = sendto(udpSocketFD, buffer, NETWORK_PACKET_SIZE, 0, (struct sockaddr*) &udpSocket, sizeof(udpSocket));

			if(sentBytes != NETWORK_PACKET_SIZE){
				//TODO: notify an error occurred
			}
		}

		break;
	case COMMAND_WRITE_SENSOR_REQUEST:
		//TODO: add impl for sensor configuration commands
		break;

	case COMMAND_READ_MOTOR_REQUEST:

		MotorOutputState motorState;

		nxtHandler = connectedDevices[(char*)pack.deviceData.deviceName];

		nxtGetMotorState(&nxtHandler, pack.deviceData.port, &motorState);

		//send response to host

		memset(&sendPack, 0, sizeof(sendPack));

		break;

	case COMMAND_WRITE_MOTOR_REQUEST:

		GenericResponse genericResp;

		nxtHandler = connectedDevices[pack.deviceData.deviceName];

#pragma pack(1)
		struct {
			uint8 power;
			uint8 mode;
			uint8 regulationMode;
			uint8 turnRatio;
			uint8 runstateMode;
			uint32 tachoLimit;
		} motorControl;

		memcpy(&motorControl, pack.deviceData.data, sizeof(motorControl));

		nxtMotorsRotate(&nxtHandler,
							pack.deviceData.port,
							motorControl.power,
							motorControl.mode,
							motorControl.regulationMode,
							motorControl.turnRatio,
							motorControl.runstateMode,
							motorControl.tachoLimit,
							1,
							&genericResp);

		if(genericResp.status){
			//TODO: notify about error
		}

		break;

	case COMMAND_INIT_SENSOR_REQUEST:

		GenericResponse resp;

		memset(&resp, 0, sizeof(resp));

		if(connectedDevices.find(pack.deviceData.deviceName) == connectedDevices.end()){
			//TODO: generate response to server with error
			break;
		}

		nxtHandler = connectedDevices[pack.deviceData.deviceName];

#pragma pack(push, 1)
		struct{
			uint8 sensorMode;
			uint8 txlen;
			uint8 rxlen;
			uint8 txdata[3];
		} initSensor;
#pragma pack(pop)

		memcpy(&initSensor, pack.deviceData.data, sizeof(initSensor));

		nxtSetInputMode(&nxtHandler, pack.deviceData.port, pack.deviceData.sensor, initSensor.sensorMode, NXT_RET, &resp);

		//if the sensor is digital the we need to configure it by using nxtLSWrite
		if(pack.deviceData.sensor == NXT_LOW_SPEED
				|| pack.deviceData.sensor == NXT_LOW_SPEED_9V
				|| pack.deviceData.sensor == NXT_HISPEED){

			nxtLSWrite(&nxtHandler, pack.deviceData.port, initSensor.txlen, 0, initSensor.txdata, 1, &resp);

			usleep(50000);

		}

		break;

	default:
		break;
	}

	usleep(pack.command.repeatDelay * 1000);

}

void ClientDaemon::keepAliveDevices(){

	if(connectedDevices.size() < 1)
		return;

	for(HashMap<string, libusb_device_handle*>::iterator it = connectedDevices.begin(); it != connectedDevices.end(); it++){
		libusb_device_handle *nxtHandler = it->second;

		nxtKeepAlive(&nxtHandler, 0, NULL);
	}
}

void ClientDaemon::announceDevices(){
	ExchangePacket sendPacket;
	int res = 0;
	uint8 buffer[128] = {0};

	memset(&sendPacket, 0, sizeof(sendPacket));

	fillSourceInfo(&sendPacket.source, config.ip, config.name, PROTOCOL_TCP);

	constructCommand(&sendPacket.command, COMMAND_DEVICE_ANNOUNCE, "", 0, 0, PROTOCOL_TCP);

	for(HashMap<string, libusb_device_handle*>::iterator it = connectedDevices.begin(); it != connectedDevices.end(); it++){
		memset(&sendPacket.deviceData, 0, sizeof(sendPacket.deviceData));
		memset(buffer, 0, sizeof(buffer));

		sendPacket.deviceData.dataFormat = FORMAT_RAW;
		memcpy(sendPacket.deviceData.deviceName, it->first.c_str(), 16);

		memcpy(buffer, &sendPacket, NETWORK_PACKET_SIZE);

		res = send(tcpSocketFD, buffer, NETWORK_PACKET_SIZE, 0);

		cout << "[CLIENT DEV ANNOUNCE][" << res << " bytes]\n";

		if(res != NETWORK_PACKET_SIZE){
			//TODO: notify an error occurred
		}

	}

}

int ClientDaemon::autoInitSensor(ExchangePacket *request, libusb_device_handle **deviceHandle){
	if(!request || !deviceHandle)
		return 1;

	GenericResponse resp;

	switch(request->deviceData.sensor){
	case NXT_SWITCH:

		return nxtSetInputMode(deviceHandle, request->deviceData.port, NXT_SWITCH, NXT_BOOLEAN_MODE, 1, &resp);

	case NXT_LIGHT_INACTIVE:
	default:
		return nxtSetInputMode(deviceHandle, request->deviceData.port, request->deviceData.sensor, NXT_RAW_MODE, 0, NULL);

	}

}
